

#include "rtdeutils.h"
#include "rtde_socket.h"
#include "rtde_hash.h"
#include "rtdebasedefine.h"
#include "cdrtdebasedefine.h"
#include <stdio.h>
#include <sys/time.h>
#include <time.h>




void build_all_hashes(RtdeHandle rtdeHandle) 
{
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        printf("build_all_hashes---RtdeHandle not found\n");
        pthread_exit(NULL);
        return ;
    }

    build_input_hash(rtdeHandle,rtde_dataspace->input_vars_num, rtde_dataspace->input_vars);
    build_output_hash(rtdeHandle,rtde_dataspace->output_vars_num, rtde_dataspace->output_vars);
    build_method_hash(rtdeHandle,rtde_dataspace->methods_num, rtde_dataspace->methods_list);
}


void *receive_thread(void *arg)
{
    RtdeHandle rtde_handle = *(RtdeHandle*)arg;
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtde_handle);
    if(rtde_dataspace == NULL)
    {
        printf("rtde_thread_init---RtdeHandle not found\n");
        pthread_exit(NULL);
        return NULL;
    }

    while(1)
    {   
        if(rtde_dataspace->rtde_recv_state == RTDE_RECV_STATE_RUN)
        {
            int rsize = socket_recv(rtde_dataspace->Socket_fd.sock_receive, rtde_dataspace->rtde_recvbuf);
            if(rsize < 0)
            {
                printf("Error: socket_recv\n");
            }    
        }
        else if(rtde_dataspace->rtde_recv_state == RTDE_RECV_STATE_PAUSE)
        {
            memset(rtde_dataspace->rtde_recvbuf, 0, sizeof(rtde_dataspace->rtde_recvbuf));
        } 
    }
}


int rtde_thread_init(RtdeHandle rtdeHandle)
{   
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        printf("rtde_thread_init---RtdeHandle not found\n");
        return -1;
    }

    //创建接收线程
    int ret = pthread_create(&(rtde_dataspace->th_receive), NULL, receive_thread, &(rtde_dataspace->rtdeHandle));
    if(ret!= 0)
    {
        printf("Error: pthread_create\n");
        return -1;
    }
    return 0;
}


int rtde_thread_destroy(RtdeHandle rtdeHandle)
{   
    RtdeDatespace_t *rtde_dataspace = get_rtde_dataspace(rtdeHandle);
    if(rtde_dataspace == NULL)
    {
        printf("rtde_thread_destroy---RtdeHandle not found\n");
        return -1;
    }
    //销毁接收线程
    pthread_cancel(rtde_dataspace->th_receive);
    return 0;
}


uint8_t rtde_get_type_len(uint8_t type)
{
    int bytelen = 0;
    switch(type) 
    {
        case INVALIDT:
            bytelen = 1;
            break;
        case BOOL:            
        case UINT8:
        case INT8:
            bytelen = 1;
            break;
        case UINT16:
        case INT16:
            bytelen = 2;
            break;
        case UINT32:
        case INT32:
        case FLOAT:
            bytelen = 4;
            break;
        case UINT64:
        case INT64:
        case DOUBLE:
            bytelen = 8;
            break;
        case VECTOR3D:
        case VECTOR6INT32:
        case VECTOR6F:
        case VECTOR6UINT32:
            bytelen = 24;
            break;
        case VECTOR6D:
            bytelen = 48;
            break;   
        case VECTOR3F:
            bytelen = 12;
            break;            
        case STRING:
            bytelen = 0xFF;       
            break;
        default:
            break;
    }
    return bytelen;
}

// 计算一个数的各位数字 ×11 之和（如 45 → 4*11 + 5*11 = 99）
static int sum_digits_times_11(int num) 
{
    int sum = 0;
    while (num > 0) {
        sum += (num % 10) * 11;  // 取最后一位 ×11
        num /= 10;               // 去掉最后一位
    }
    return sum;
}

// 获取当前时间，并计算月、日、时、分、秒、毫秒的 ×11 之和
int get_time_multiply11_sum() 
{
    struct timeval tv;
    struct tm *tm_info;

    // 获取当前时间（精确到毫秒）
    gettimeofday(&tv, NULL);
    tm_info = localtime(&tv.tv_sec);

    // 提取月、日、时、分、秒、毫秒
    int month = tm_info->tm_mon + 1;      // 月份范围 1-12
    int day = tm_info->tm_mday;
    int hour = tm_info->tm_hour;
    int minute = tm_info->tm_min;
    int second = tm_info->tm_sec;
    int millisecond = tv.tv_usec / 1000;  // 微秒 → 毫秒

    // 计算每个部分的 ×11 之和
    int sum = 0;
    sum += sum_digits_times_11(month);
    sum += sum_digits_times_11(day);
    sum += sum_digits_times_11(hour);
    sum += sum_digits_times_11(minute);
    sum += sum_digits_times_11(second);
    sum += sum_digits_times_11(millisecond);

    return sum;
}



// int rtde_get_control_version(uint16_t *model, uint8_t* firmware_version, uint8_t* hardware_version, uint8_t* bootloader_version)
// {
//     *(uint16_t *)&rtde_ioctrl_sendbuf[0] = 3;
//     rtde_ioctrl_sendbuf[2] = RTDE_GET_CONTROL_VERSION;
    
//     int ssize = socket_send(Socket_fd.sock_io_control, rtde_ioctrl_sendbuf, *(uint16_t *)&rtde_ioctrl_sendbuf[0]);
//     int rsize = socket_recv(Socket_fd.sock_io_control, rtde_ioctrl_recvbuf);

// #if DEBUG_PRINT
//     for(int i=0; i<rsize; i++)
//     {
//         printf("%02x ", rtde_ioctrl_recvbuf[i]);
//     }    
//     printf("\n");
// #endif

//     if(rtde_ioctrl_recvbuf[3]!= 0)//result != 0
//     {
//         printf("Error: %d\n", rtde_ioctrl_recvbuf[3]);
//         return -1;
//     }

//     int pos = 7;//skip header+result,7bytes

//     int slen;
//     char sname[100];
//     *model = *(uint16_t *)&rtde_ioctrl_recvbuf[pos]; 
//     pos += 2;

//     slen = rtde_ioctrl_recvbuf[pos];  //firmware_version
//     pos++;
//     memcpy(firmware_version, &rtde_ioctrl_recvbuf[pos], slen); 
//     pos += slen;

//     slen = rtde_ioctrl_recvbuf[pos];  //hardware_version
//     pos++;
//     memcpy(hardware_version, &rtde_ioctrl_recvbuf[pos], slen); 
//     pos += slen;

//     slen = rtde_ioctrl_recvbuf[pos];  //bootloader_version
//     pos++;
//     memcpy(bootloader_version, &rtde_ioctrl_recvbuf[pos], slen); 
//     pos += slen;

//     return 0;
// }




    // //                    double*6             double*6
    // char* varNames1 = "actualJointPosition,actualJointVelocity";
    // //                    double*6          double*6
    // char* varNames2 = "actualJointCurrent,actualJointVoltage";
    // //                    double*6      uint32*6
    // char* varNames3 = "jointTemperature,jointMode";//jointMode:
    // //                      double                   double
    // char* varNames4 = "actualExJointPosition1,actualExJointVelocity1";
    // //                    double*6              double*6
    // char* varNames5 = "targetJointPosition,targetJointVelocity";
    // //                    double*6              double              double
    // char* varNames6 = "targetJointCurrent,targetExJointPosition1,targetExJointVelocity1";
    // //                    double*6        double*6
    // char* varNames7 = "tcpVectorActual,tcpSpeedActual";
    // //                    double*6        double*6
    // char* varNames8 = "tcpVectorTarget,tcpSpeedTarget";
    // //                   double*6        double*6
    // char* varNames9 = "sensorForceRaw,sensorForceAdmt";
    // //                    double    double    double    double  double   
    // char* varNames10 = "robotPower,robotWork,mechPower,mechWork,momentum";

    // char* varNames11 = "masterboardTemperature,safetyboardTemperature,masterboardVoltage";

    // char* varNames12 = "robotVoltage,robotCurrent,masterboardPower,masterIOCurrent";



